/*
 * This file is part of the Sx Framework Library.
 * 
 * Copyright (C) 2013 University of Colorado Denver
 * <min.choi@ucdenver.edu> <shane.transue@ucdenver.edu>
 * 
 * Permission is hereby granted, free of charge, to any person obtaining a copy 
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 
 * copies of the Software, and to permit persons to whom the Software is 
 * furnished to do so, subject to the following conditions:
 * 
 * The above copyright notice and this permission notice shall be included in 
 * all copies or substantial portions of the Software.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 
 * DEALINGS IN THE SOFTWARE.
 */
#ifndef SX_MATH_H
#define SX_MATH_H

/*
 * The Eigen templated linear algebra library provides most of the basic types
 * utilized in the Math library.
 */
#include <Eigen/Eigen>
#include <Eigen/StdVector>
#include <iostream>

namespace Sx {
namespace Math {

typedef double Real;

/*
 * Transform provides the type of transforms that can be applied to a
 * transformation matrix. These are typically used for determining the 
 * type of transform that should be applied to a rigid body object.
 */
enum Transform { TRANSLATE, ROTATE, SCALE, SHEAR, NO_TRANSFORM };

/*
 * Utilizing a perspective projection there are two different camera
 * perspectives that can be used; first persona and third person. These
 * can be used with camera operations and configurations.
 */
enum Perspective { FIRST_PERSON, THIRD_PERSON, NO_PERSPECTIVE };

/*
 * Two common projections in 3d modeling applications are perspective and
 * orthographic.
 */
enum Projection { PERSPECTIVE, ORTHOGRAPHIC, NO_PROJECTION };

/*
 * Provides an enumeration of all 6 possible orthographic views locked to
 * the major axes.
 */
enum View { TOP, BOTTOM, LEFT, RIGHT, FRONT, BACK, NO_VIEW };

/*
 * A simple enumeration of all major axis directions.
 * X, Y, Z represent the positive directions.
 * NX, NY, NZ represent the negative directions.
 */
enum Axis { X, Y, Z, NX, NY, NZ, NO_AXIS };

/*
 * Provides an enumeration of all possible major planes (locked to the major
 * axes).
 */
enum AxisPlane { XY, XZ, YZ, NO_AXISPLANE };

/*
 * Coordinate operations can take place in local coordinates or global 
 * coordinates. These are used for utilizing manipulations in different ways.
 */
enum CoordinateSpace { LOCAL, GLOBAL };

/*
 * Provides an enumeration of the basic types of geometric primitives.
 */
enum PrimitiveType { POINT, VERTEX, LINE, SEGMENT, TRIANGLE, FACE, QUAD, POLYGON, TRIANGLE_FAN };

/*
 * PI Constants.
 */
const static float TWO_PI = 6.283185307179586f;
const static float PI = 3.141592653589793f;
const static float HALF_PI = 1.570796326794896f;
const static float QUARTER_PI = 0.785398163f;

const static float EPSILON = 1.0E-10f;
/*
 * The following constants and functions are used to convert Degrees to
 * Radians and Radians to Degrees.
 */
const static float DEGREE_180 = 180.0f;
const static float DegToRadConv = static_cast<float>(M_PI) / DEGREE_180;
const static float RadToDegConv = DEGREE_180 / static_cast<float>(M_PI);

inline float DegreesToRadians(float degrees) {
	return degrees * DegToRadConv;
}

inline float RadiansToDegrees(float radians) {
	return radians * RadToDegConv;
}

inline bool FloatCompare(float valueA, float valueB, float epsilon = EPSILON) {
	if ( valueB > valueA - epsilon && valueB < valueA + epsilon ) return true;
	return false;
}

inline bool DoubleCompare(double valueA, double valueB, double epsilon = EPSILON) {
	if ( valueB > valueA - epsilon && valueB < valueA + epsilon ) return true;
	return false;
}

inline bool InInterval(float intervalStart, float intervalEnd, float x, bool inclusive = true) {
	if ( inclusive ) {
		if ( x < intervalStart || x > intervalEnd ) return false;
		else return true;
	}
	else {
		if ( x <= intervalStart || x >= intervalEnd ) return false;
		else return true;
	}

	return false;
}

/*
 * Assuming that vFOV is provided in Degrees. This is due to the argument value
 * specified for the gluPerspective function which takes the vertical FOV as
 * degrees.
 */
inline float VerticalFOVtoHorizontalFOV(float vFOV, float aspect) {
	float vFOVr = DegreesToRadians(vFOV);
	float hFOV = 2.0f * std::atan(std::tan(vFOVr * 0.5f) * aspect);
	return RadiansToDegrees(hFOV);
}

/*
 * Assuming that hFOV is provided in Degrees. This is due to the argument value
 * specified for the gluPerspective function which takes the vertical FOV as
 * degrees.
 */
inline float HorizontalFOVtoVerticalFOV(float hFOV, float aspect) {
	float hFOVr = DegreesToRadians(hFOV);
	float vFOV = 2.0f * std::atan(std::tan(hFOVr * 0.5f) * aspect);
	return RadiansToDegrees(vFOV);
}

/* Snaps the provided value to the closest increment of spacing */
inline float Snap(float spacing, float value) {
	if ( spacing == 0.0f ) return 0.0f;
    float left = spacing * std::floor(value / spacing);
    float right = left + spacing;
    if ((value - left) < (right - value)) return left;
    else return right;
}

inline double SnapLeft(double spacing, double value) {
	if ( spacing == 0.0 ) return 0.0;
	return spacing * std::floor(value / spacing);
}

inline double SnapRight(double spacing, double value) {
	if ( spacing == 0.0 ) return spacing;
	double left = spacing * std::floor(value / spacing);
	return left + spacing;
}

/* Snaps the provided value to the closest increment of spacing */
inline double Snap(double spacing, double value) {
	if ( spacing == 0.0f ) return 0.0;
    double left = spacing * std::floor(value / spacing);
    double right = left + spacing;
    if ((value - left) < (right - value)) return left;
    else return right;
}

/* Returns the snap location of the provided point. */
inline Eigen::Vector2f Snap2D(float uSpacing, float vSpacing, const Eigen::Vector2f& point) {
	Eigen::Vector2f snapped;
	snapped.x() = Snap(uSpacing, point.x());
	snapped.y() = Snap(vSpacing, point.y());
	return snapped;
}

/* Returns the snap location of the provided point. */
inline Eigen::Vector3f Snap3D(float xSpacing, float ySpacing, float zSpacing, const Eigen::Vector3f& point) {
	Eigen::Vector3f snapped;
	snapped.x() = Snap(xSpacing, point.x());
	snapped.y() = Snap(ySpacing, point.y());
	snapped.z() = Snap(zSpacing, point.z());
	return snapped;
}

/* This function calculates the distance between two Vector2fs. */
inline float Distance(const Eigen::Vector2f& a, const Eigen::Vector2f& b) {
	return std::sqrt(std::pow(a.x() - b.x(), 2.0f) + std::pow(a.y() - b.y(), 2.0f));
}

/* This function calculates the distance between two Vector2is. */
inline float Distance(const Eigen::Vector2i& a, const Eigen::Vector2i& b) {
	return std::sqrt(std::pow(static_cast<float>(a.x() - b.x()), 2.0f) + std::pow(static_cast<float>(a.y() - b.y()), 2.0f));
}

/* This function calculates the distance between two Vector3fs. */
inline float Distance(const Eigen::Vector3f& a, const Eigen::Vector3f& b) {
	return std::sqrt(std::pow(a.x() - b.x(), 2.0f) + std::pow(a.y() - b.y(), 2.0f) + std::pow(a.z() - b.z(), 2.0f));
}

/* This function calculates the distance between two Vector3fs. */
inline double Distance(const Eigen::Vector3d& a, const Eigen::Vector3d& b) {
	return std::sqrt(std::pow(a.x() - b.x(), 2.0) + std::pow(a.y() - b.y(), 2.0) + std::pow(a.z() - b.z(), 2.0));
}

/*
 * This function applies a Matrix4f to a Vector3f.
 */
inline Eigen::Vector3f ApplyTransform(const Eigen::Matrix4f& transform, const Eigen::Vector3f& vector) {
	Eigen::Vector4f v4(vector.x(), vector.y(), vector.z(), 1.0f);
	transform.applyThisOnTheLeft(v4);
	return Eigen::Vector3f(v4.x(), v4.y(), v4.z());
}

inline Eigen::Vector3d ApplyTransform(const Eigen::Matrix4f& transform, const Eigen::Vector3d& vector) {
	Eigen::Vector4f v4(static_cast<float>(vector.x()), static_cast<float>(vector.y()), static_cast<float>(vector.z()), 1.0f);
	transform.applyThisOnTheLeft(v4);
	return Eigen::Vector3d(v4.x(), v4.y(), v4.z());
}

/*
 * From the provided triangle (as a set of three points: a, b, c) the normalized
 * normal to the surface of the triangle is calculated and returned.
 */
inline Eigen::Vector3f Normal(const Eigen::Vector3f& a, const Eigen::Vector3f& b, const Eigen::Vector3f& c) {
	Eigen::Vector3f A = b - a;
	Eigen::Vector3f B = c - a;
	return A.cross(B).normalized();
}

inline Eigen::Vector3d Normal(const Eigen::Vector3d& a, const Eigen::Vector3d& b, const Eigen::Vector3d& c) {
	Eigen::Vector3d A = b - a;
	Eigen::Vector3d B = c - a;
	return A.cross(B).normalized();
}

/*
 * Uses the parametric equation of a line in 3D space to create a line segment.
 * The length of the line segment is given by extent.
 */
inline bool LineFromParametric(const Eigen::Vector3f& origin, const Eigen::Vector3f& direction, float extent, Eigen::Vector3f& p0, Eigen::Vector3f& p1) {
	if ( direction == Eigen::Vector3f::Zero() ) {
		std::cerr << "[Math:LineFromParametric] Error: Directional vector cannot be a zero vector." << std::endl;
		std::cerr << "  Provided p0 and p1 points unmodified." << std::endl;
		return false;
	}

	Eigen::Vector3f nDir = direction.normalized();

	p0 = origin;
	p1 = (origin + (extent * nDir));
	return true;
}

inline bool LineFromParametric(const Eigen::Vector3d& origin, const Eigen::Vector3d& direction, double extent, Eigen::Vector3d& p0, Eigen::Vector3d& p1) {
	if ( direction == Eigen::Vector3d::Zero() ) {
		std::cerr << "[Math:LineFromParametric] Error: Directional vector cannot be a zero vector." << std::endl;
		std::cerr << "  Provided p0 and p1 points unmodified." << std::endl;
		return false;
	}

	Eigen::Vector3d nDir = direction.normalized();

	p0 = origin;
	p1 = (origin + (extent * nDir));
	return true;
}

/* Basic Rounding function. */
inline float Round(float f) {
	return std::floor(f + 0.5f);
}

/*
 * Given a distance, theta, and phi angles the three dimensional coordinate of
 * the point is calculated. The distance represents the distance between the
 * calculated point and the origin. The theta angle is measured about the Y
 * axis. The angle Phi represents an incline angle which rotates about an axis
 * as described below:
 *	Take directional vector d-> and rotate it about the Y axis by theta.
 *	Take the cross product between the resulting rotated d-> vector and the Y
 *	axis. This will be the axis of rotation for the amount of phi.
 * Angles should be provided in radians.
 */
inline Eigen::Vector3f CalculatePerspectiveCoordinate(float distance, float theta, float phi) {
	float bd = distance * std::cos(phi);
	float x = bd * sin(theta);
	float y = distance * std::sin(phi);
	float z = bd * cos(theta);
	return Eigen::Vector3f(x, y, z);
}

/*
 * See CalculatePerspectiveCoordinate. The angles are provided in degrees.
 */
inline Eigen::Vector3f CalculatePerspectiveCoordinateDeg(float distance, float theta, float phi) {
	float thetaDeg = DegreesToRadians(theta);
	float phiDeg = DegreesToRadians(phi);

	float bd = distance * std::cos(phiDeg);
	float x = bd * sin(thetaDeg);
	float y = distance * std::sin(phiDeg);
	float z = bd * cos(thetaDeg);
	return Eigen::Vector3f(x, y, z);
}

/* Convert Cartesian coordinates to cylindrical coordinates. */
inline Eigen::Vector3f CylindricalToCartesian(const Eigen::Vector3f& p) {
	Eigen::Vector3f result;
	result.x() = p.x() * std::cos(p.y());
	result.y() = p.x() * std::sin(p.y());
	result.z() = p.z();
	return result;
}

/* Convert cylindrical coordinates to Cartesian coordinates. */
inline Eigen::Vector3f CylindricalToCartesian(float rho, float phi, float z) {
	Eigen::Vector3f result;
	result.x() = rho * std::cos(phi);
	result.y() = rho * std::sin(phi);
	result.z() = z;
	return result;
}

/* Given a mathematical system with either Y or Z axis as the primary
 * vertical axis, swap these components to convert one to the other.
 */
inline void SwapUpVector(Eigen::Vector3f& v) {
	float tmp = v.y();
	v.y() = v.z();
	v.z() = tmp;
}

/* Return the angle between two vectors. */
inline float AngleBetweenVectors(const Eigen::Vector3f& a, const Eigen::Vector3f& b) {
	return std::acos((a.x()*b.x() + a.y()*b.y() + a.z()*b.z()) / (a.norm() * b.norm()));
}

inline double AngleBetweenVectors(const Eigen::Vector3d& a, const Eigen::Vector3d& b) {
	return std::acos((a.x()*b.x() + a.y()*b.y() + a.z()*b.z()) / (a.norm() * b.norm()));
}

/*
 * Given a homogeneous transformation matrix, extract the
 * rotational matrix and return it as a 3x3 rotation matrix.
 */
inline Eigen::Matrix3f extractRotationMatrix(const Eigen::Matrix4f& transform) {
	Eigen::Matrix3f rotationMatrix;
	for ( unsigned int i = 0; i < 3; i++ ) {
		for ( unsigned int j = 0; j < 3; j++ )  {
			rotationMatrix(i, j) = transform(i, j);
		}
	}
	return rotationMatrix;
}

/*
 * Given a homogeneous transformation matrix, extract the positional
 * information and return it as a vector 3.
 */
inline Eigen::Vector3f extractPosition(const Eigen::Matrix4f& transform) {
	Eigen::Vector3f position;
	position.x() = transform(0, 3);
	position.y() = transform(1, 3);
	position.z() = transform(2, 3);
	return position;
}

inline void Lerp(const Eigen::Vector3f& a, const Eigen::Vector3f& b, float t, Eigen::Vector3f& p) {
	p.x() = a.x() + (b.x() - a.x()) * t;
	p.y() = a.y() + (b.y() - a.y()) * t;
	p.z() = a.z() + (b.z() - a.z()) * t;
}

inline Eigen::Vector3f Interp(const Eigen::Vector3f& p1, const Eigen::Vector3f& p2, float t) {
	return p1 * (1 - t) + p2 * t;
}

inline Eigen::Vector3d Interp(const Eigen::Vector3d& p1, const Eigen::Vector3d& p2, double t) {
	return p1 * (1 - t) + p2 * t;
}

typedef Eigen::Matrix<float, 4, 4, Eigen::DontAlign> Matrix4f;
typedef Eigen::Matrix<float, 3, 3, Eigen::DontAlign> Matrix3f;
typedef Eigen::Matrix<float, 3, 1, Eigen::DontAlign> Vector3f;
typedef Eigen::Quaternion<float, Eigen::DontAlign> Quaternionf;

}

}

#endif
